#include "mex.h"
#include <stdio.h>
#include <stdlib.h>

enum {messageIdx                  = 0, /* input code */
      llrVectorIdx,
      numInputsIdx,       /* number of input bits    */
      numOutputsIdx,       /* number of output bits   */
      numStatesIdx,      /* number of states        */
      nxtStatesIdx,          /* output matrix (decimal) */
      nxtOutputIdx,      /* next state matrix       */
      codeRateIdx
      };
      
#define messageArg                 (prhs[messageIdx])
#define llrVectorArg               (prhs[llrVectorIdx])
#define numInputsArg               (prhs[numInputsIdx])
#define numOutputsArg              (prhs[numOutputsIdx])
#define numStatesArg               (prhs[numStatesIdx])
#define nxtStatesArg               (prhs[nxtStatesIdx])
#define nxtOutputArg               (prhs[nxtOutputIdx])
#define codeRateArg                (prhs[codeRateIdx])
#define output                  (plhs[0])

#define INF 9E+9

static void CheckParameters(int nlhs,       mxArray *plhs[],
                            int nrhs, const mxArray *prhs[])
{
    const char  *msg   = NULL;

    /* Check number of parameters */
    if (!(nrhs == 8)) {
        msg = "Invalid number of input arguments. This function only works"
        "with 8 input arguments.";
        goto EXIT_POINT;
    }
    if(nlhs >= 2){
        msg = "Invalid number of output arguments. This function only works"
        "with 1 output argument.";
        goto EXIT_POINT;
    }


EXIT_POINT:
    if(msg != NULL){
        mexErrMsgTxt(msg);
    }
}

static void SovaDecoder(int nlhs,       mxArray *plhs[],
                       int nrhs, const mxArray *prhs[])
{
        // Starting the SOVA:
   double *message,*llrVector,*nxtStates,*nxtOutput,*out,*metric,*competitor,
   *survivor,*delta,*input,*inputCompetitor,*soft, codeRate; 
   int numStates,steps,counter, maximMetricState,outputIndex;
   
   message = mxGetPr(messageArg);
   llrVector = mxGetPr(llrVectorArg);
   numStates = (int)mxGetScalar(numStatesArg);
   nxtStates = mxGetPr(nxtStatesArg);
   nxtOutput = mxGetPr(nxtOutputArg);
   codeRate = (double)mxGetScalar(codeRateArg);
   steps =  (mxGetN(llrVectorArg))/(mxGetScalar(numInputsArg));
   
   output = mxCreateDoubleMatrix(1,steps,mxREAL);
   out = mxGetPr(output);
   
   metric = (double*)malloc(numStates*(steps+1)*sizeof(double));
   competitor = (double*)malloc(numStates*(steps+1)*sizeof(double));
   survivor = (double*)malloc(numStates*(steps+1)*sizeof(double));
   delta = (double*)malloc(numStates*(steps+1)*sizeof(double));
   input = (double*)malloc(numStates*(steps+1)*sizeof(double));
   inputCompetitor = (double*)malloc(numStates*(steps+1)*sizeof(double));
   soft = (double*)malloc((steps)*sizeof(double));
   
   
   for (counter = 0; counter < numStates * ( steps + 1 ) ; counter++){
       metric[counter] = -INF;
   }
   
   for(counter = 0; counter < steps; counter++){
       soft[counter] = INF;
   }
   
   for(counter = 0; counter<numStates*2;  counter++){
       survivor[counter] = 0;
       competitor[counter]=0;
       delta[counter] = 0;
       input[counter] = 0;
       inputCompetitor[counter] = 0;   
   }
   
   metric[0]=0;
   
  for(counter = 0; counter<steps; counter++){
      
      int state,inputSymbol,symbol,nextState,outputSymbol, auxiliar,aux2;
      double auxiliarMetric, difference, *inputBlock,*llrBlock;
      
      int offset = counter*mxGetScalar(numOutputsArg);
      
      inputBlock = message + offset;
      llrBlock = llrVector + counter;
      
        for(aux2=0; aux2<numStates; aux2++){
                metric[aux2+(counter+1)*numStates] = -2*INF;
             }
      
      for(state=0; state<numStates; state++){
          for (inputSymbol = 0; inputSymbol<2; inputSymbol++){
             
           
              nextState = nxtStates [ state + inputSymbol * numStates ];
              outputSymbol = nxtOutput [ state + inputSymbol * numStates ];
              symbol = 2 * inputSymbol - 1;
              auxiliarMetric = 0;
              
              for(auxiliar = 0; auxiliar<2; auxiliar++){
                  auxiliarMetric = auxiliarMetric +
                        inputBlock[1-auxiliar]*((outputSymbol & 01)*2-1);
                  outputSymbol >>= 1;
              }
              
              auxiliarMetric = auxiliarMetric + metric [state + counter*numStates]
                        + symbol*(llrBlock[0])*codeRate;
              
              if (metric [nextState + (counter + 1 ) * numStates ] < auxiliarMetric ){
                  
                  delta [ nextState + (counter + 1) * numStates] = auxiliarMetric-
                            metric [ nextState + (counter + 1 ) * numStates];

                  competitor[ nextState + (counter + 1) * numStates ] =
                            survivor[ nextState + (counter + 1) * numStates ];
                  
                  inputCompetitor [ nextState + (counter + 1) * numStates ]= 
                            input [ nextState + (counter + 1) * numStates ];
                  
                  metric[ nextState + (counter + 1) * numStates ] = auxiliarMetric;
                  survivor[ nextState + (counter + 1) * numStates ] = state;


                  input [ nextState + (counter + 1) * numStates ] = inputSymbol;
                  
              }else {
                  difference = metric [ nextState + (counter + 1 ) * numStates] - 
                            auxiliarMetric;

                  if(difference <= delta[ nextState + (counter + 1) * numStates ]){

                      delta[ nextState + (counter + 1) * numStates ] = difference;
                      
                      competitor[ nextState + (counter + 1) * numStates ] = state;
                      
                      inputCompetitor[ nextState + (counter + 1) * numStates ] = inputSymbol;
                  }
              }
          }
      }
  }
   
    maximMetricState = 0;
    for(counter = 1; counter<numStates; counter++){
       if(metric[counter+steps*numStates] > metric[ maximMetricState + steps*numStates]){
            maximMetricState = counter;
       }
    }
    
    outputIndex = steps-1;
    for(counter = steps; counter>0; counter--){
        double difference; 
        int competitorState,survivorState,auxiliar,inputBit, competitorBit;
        
        inputBit = input[maximMetricState+counter*numStates];
        out[counter-1] = inputBit;
        competitorBit = inputCompetitor[maximMetricState+counter*numStates];
        competitorState = competitor[maximMetricState+counter*numStates];
        difference = delta[maximMetricState+counter*numStates];
        survivorState = survivor[maximMetricState+counter*numStates];

        for(auxiliar = 0; auxiliar<counter; auxiliar++){

            if (inputBit != competitorBit){
                
                if (difference < soft[counter-auxiliar-1]){
                    soft[counter-auxiliar-1] = difference;
                }
            }
            if( survivorState == competitorState) { break; }
            
            inputBit = input[survivorState+numStates*(counter-1-auxiliar)];
            competitorBit = input[competitorState+numStates*(counter-1-auxiliar)];
            survivorState= survivor[survivorState+numStates*(counter-1-auxiliar)];
            competitorState = survivor[competitorState + numStates*(counter-1-auxiliar)];
            
            
            
        }
        
        maximMetricState = survivor[maximMetricState + counter*numStates];
    }
    
    for(counter=0; counter < steps; counter++ ){
        out[counter] = (2*out[counter]-1)*soft[counter];
    }
    
   free(metric);
   free(competitor);
   free(survivor);
   free(delta);
   free(inputCompetitor);
   free(soft);
   free(input);
}
void mexFunction(int nlhs,       mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
    CheckParameters(nlhs, plhs, nrhs, prhs);
    SovaDecoder(nlhs,plhs, nrhs,prhs);
    

}
